#include "transform.h"
#include "geometry.h"

bool Transform::SwapHandedness() const {
	float det = 
		(
			(mat.m11 * (mat.m22 * mat.m33 - mat.m23 * mat.m32)) -
			(mat.m12 * (mat.m21 * mat.m33 - mat.m23 * mat.m31)) +
			(mat.m13 * (mat.m21 * mat.m32 - mat.m22 * mat.m31))
		);
	return det < 0.f;
}
Transform Translate(float tx, float ty, float tz)
{
	return Transform
		(
			Matrix4x4
			(
				1, 0, 0, tx,
				0, 1, 0, ty,
				0, 0, 1, tz,
				0, 0, 0, 1
			),
			Matrix4x4
			(
				1, 0, 0, -tx,
				0, 1, 0, -ty,
				0, 0, 1, -tz,
				0, 0, 0, 1
			)
		);
}
Transform Translate(const Vector3 &v)
{
	return Transform
		(
		Matrix4x4
		(
			1, 0, 0, v.x,
			0, 1, 0, v.y,
			0, 0, 1, v.z,
			0, 0, 0, 1
		),
		Matrix4x4
		(
			1, 0, 0, -v.x,
			0, 1, 0, -v.y,
			0, 0, 1, -v.z,
			0, 0, 0, 1
		)
		);
}
Transform Scale(float sx, float sy, float sz)
{
	return Transform
		(
			Matrix4x4
			(
				sx, 0, 0, 0,
				0, sy, 0, 0,
				0, 0, sz, 0,
				0, 0, 0, 1
			),
			Matrix4x4
			(
				1.f/sx,	0,		0,		0,
				0,		1.f/sy,	0,		0,
				0,		0,		1.f/sz,	0,
				0,		0,		0,		1
			)
		);
}
Transform RotateX(float degree)
{
	float sint = sinf(Radians(degree));
	float cost = cosf(Radians(degree));
	return Transform
		(
			Matrix4x4
			(
				1,	0,		0,		0,
				0,	cost,	-sint,	0,
				0,	sint,	cost,	0,
				0,	0,		0,		1
			),
			Matrix4x4
			(
				1,	0,		0,		0,
				0,	cost,	sint,	0,
				0,	-sint,	cost,	0,
				0,	0,		0,		1
			)
		);
}
Transform RotateY(float degree)
{
	float sint = sinf(Radians(degree));
	float cost = cosf(Radians(degree));
	return Transform
		(
			Matrix4x4
			(
				cost,	0,	sint,	0,
				0,		1,	0,		0,
				-sint,	0,	cost,	0,
				0,		0,	0,		1
			),
			Matrix4x4
			(
				cost,	0,	-sint,	0,
				0,		1,	0,		0,
				sint,	0,	cost,	0,
				0,		0,	0,		1
			)
		);
}
Transform RotateZ(float degree)
{
	float sint = sinf(Radians(degree));
	float cost = cosf(Radians(degree));
	return Transform
		(
			Matrix4x4
			(
				cost,	-sint,	0,		0,
				sint,	cost,	0,		0,
				0,		0,		1,		0,
				0,		0,		0,		1
			),
			Matrix4x4
			(
				cost,	sint,	0,		0,
				-sint,	cost,	0,		0,
				0,		0,		1,		0,
				0,		0,		0,		1
			)
		);
}
Transform Rotate(float degree, float rx, float ry, float rz)
{
	// make sure axis is normalized
	Vector3 axis(rx,ry,rz);
	axis = Normalize(axis);

	float sint = sinf(Radians(degree));
	float cost = cosf(Radians(degree));
	
	Matrix4x4 m;
	
	m.m11 = axis.x * axis.x + (1.f - axis.x * axis.x) * cost;
	m.m12 = axis.x * axis.y * (1.f - cost) - axis.z * sint;
	m.m13 = axis.x * axis.z * (1.f - cost) + axis.y * sint;
	m.m14 = 0;

	m.m21 = axis.x * axis.y * (1.f - cost) + axis.z * sint;
	m.m22 = axis.y * axis.y + (1.f - axis.y * axis.y) * cost;
	m.m23 = axis.y * axis.z * (1.f - cost) - axis.x * sint;
	m.m24 = 0;

	m.m31 = axis.x * axis.z * (1.f - cost) - axis.y * sint;
	m.m32 = axis.y * axis.z * (1.f - cost) + axis.x * sint;
	m.m33 = axis.z * axis.z + (1.f - axis.z * axis.z) * cost;
	m.m34 = 0;

	m.m41 = 0;
	m.m42 = 0;
	m.m43 = 0;
	m.m44 = 1;

	// The matrix is orthonormal 
	// Inverse(M) = Transpose(M) 
	return Transform(m, m.Transpose());

}
Transform Rotate(float degree, const Vector3 &v)
{
	Vector3 axis = v;
	axis = Normalize(axis);

	float sint = sinf(Radians(degree));
	float cost = cosf(Radians(degree));

	Matrix4x4 m;

	m.m11 = axis.x * axis.x + (1.f - axis.x * axis.x) * cost;
	m.m12 = axis.x * axis.y * (1.f - cost) - axis.z * sint;
	m.m13 = axis.x * axis.z * (1.f - cost) + axis.y * sint;
	m.m14 = 0;

	m.m21 = axis.x * axis.y * (1.f - cost) + axis.z * sint;
	m.m22 = axis.y * axis.y + (1.f - axis.y * axis.y) * cost;
	m.m23 = axis.y * axis.z * (1.f - cost) - axis.x * sint;
	m.m24 = 0;

	m.m31 = axis.x * axis.z * (1.f - cost) - axis.y * sint;
	m.m32 = axis.y * axis.z * (1.f - cost) + axis.x * sint;
	m.m33 = axis.z * axis.z + (1.f - axis.z * axis.z) * cost;
	m.m34 = 0;

	m.m41 = 0;
	m.m42 = 0;
	m.m43 = 0;
	m.m44 = 1;

	// The matrix is orthonormal 
	// Inverse(M) = Transpose(M) 
	return Transform(m, m.Transpose());
}
Transform FromFrame(Vector3 s, Vector3 t, Vector3 n)
{
	Vector3 ss = Normalize(s);
	Vector3 tt = Normalize(t);
	Vector3 nn = Normalize(n);
	Matrix4x4 m;
	/*
		ss.x	tt.x	nn.x	0
		ss.y	tt.y	nn.y	0
		ss.z	tt.z	nn.z	0
		0		0		0		1	
	*/
	m.SetColumn1(ss.x, ss.y, ss.z, 0);
	m.SetColumn2(tt.x, tt.y, tt.z, 0);
	m.SetColumn3(nn.x, nn.y, nn.z, 0);
	m.SetColumn4(0, 0, 0, 1);

	// The matrix is orthonormal 
	// Inverse(M) = Transpose(M) 
	return Transform(m,m.Transpose());
}
Transform LookAt(const Point3 &eye, const Point3 &center, const Vector3 &up)
{
	// Here we use left-handed coordinate
	// Cross should be perform with left-hand rule
	// http://msdn.microsoft.com/en-us/library/bb281710(v=vs.85).aspx

	Vector3 dir = Normalize(center - eye);					// z - t
	Vector3 right = Normalize(Cross(Normalize(up), dir));	// x - u
	Vector3 newUp = Normalize(Cross(dir, right));			// y - v
	
	Matrix4x4 m;
	m.SetColumn1(right.x, newUp.x, dir.x, 0);
	m.SetColumn2(right.y, newUp.y, dir.y, 0);
	m.SetColumn3(right.z, newUp.z, dir.z, 0);
	m.SetColumn4(-Dot(right, Vector3(eye)), -Dot(newUp, Vector3(eye)), -Dot(dir, Vector3(eye)), 1);

	return Transform(m, m.Inverse());
}
Transform Perspective(float fov, float clipNear, float clipFar)
{
	/* Project vectors in camera space onto a plane at z=1:
	 *
	 *  xProj = x / z
	 *  yProj = y / z
	 *  zProj = (far * (z - near)) / (z * (far-near))
	 *  
	 *  Camera-space depths are not mapped linearly!
	 */

	float recip = 1.0f / (clipFar - clipNear);

	Matrix4x4 m(
		1,   0,   0,   0,
		0,   1,   0,   0,
		0,   0,   clipFar * recip, -clipNear * clipFar * recip,
		0,   0,   1,   0
	);

	// Perform a scale so that the field of view is mapped to the interval [-1, 1]
	// height = [-1, 1]
	float cot = 1.0f / std::tanf(Radians(fov/2.0f));

	return Scale(cot, cot, 1.0f) * Transform(m);
}
Transform Orthographic(float clipNear, float clipFar)
{
	/************************************************************************/
	/* I think this one is better, since height has to be mapped to [-1,1]  */
	/* but the implementation below works well in pbrt						*/
	/************************************************************************/
	// Maybe like this
	// return Scale(1/w,1/h,1)*Scale(1.f, 1.f, 1.f / (clipFar-clipNear)) * Translate(0.f, 0.f, -clipNear);

	// z is mapped to the interval [0, 1]
	return Scale(1.f, 1.f, 1.f / (clipFar-clipNear)) *
		Translate(0.f, 0.f, -clipNear);
}